#pragma once
#include <ctime>
#include "MSTCOSSDK.h"
#include "TPoint.h"

float fitAngel(float x) {
	while (x < -180) x+=360;
	while (x > 180) x-=360;
	return x;
}

TPoint fitEdge(TPoint x) {
	return TPoint(
		min(max(x.x, -MapWidth/2), MapWidth/2),
		min(max(x.y, -MapHeight/2), MapHeight/2));
}

template<typename T>
int findMinIdx(TPoint x, const vector<T>& vec) {
	float minDis = FLT_MAX;
	int minIdx = -1;
	for (unsigned int i=0; i<vec.size(); ++i) {
		float dis = (x - vec[i].pos).norm();
		if (minDis < dis) {
			minDis = dis;
			minIdx = i;
		}
	}
	return minIdx;
}

float nowTime(void) {
	return (0.0f+clock())/CLOCKS_PER_SEC;
}